#include "TPSN2.h"
#include "Atm128Interrupt.h"
#include "printf.h" // ignore red line its not an error

#define LEVEL_LOCK_TIME				2000
#define MIN_TIME_BETWEEN_RESYNC 	2000

#define WAIT_RANDOM_TIME 			500		// The wait period is a random number between 0 and WAIT_TIME
#define SYNC_TIMEOUT 				700		// If no ACK is received a timeout will occur. This will happen after a period of between (SYNC_TIMEOUT-SYNC_TIMEOUT/3) to (SYNC_TIMEOUT+SYNC_TIMEOUT/3)
#define BACK_OFF_TIME 				1410	// When a node receives a syncPulse from a -1 level lower node a backoff will occur. Backoff last between (BACK_OFF_TIME-BACK_OFF_TIME/3) and (BACK_OFF_TIME+BACK_OFF_TIME/3)
											// - BACK_OFF_TIME should be chosen a little higher than SYNC_TIMEOUT x 2.												  
#define LEVEL_REQUEST_TIMEOUT 		1500
#define INTERFACE_TIMEOUT1  		1500
#define INTERFACE_TIMEOUT2 			1200	// ændret fra 700
#define RESYNC_PERIODIC_TIMER 		1250
#define JOIN_NETWORK_TIMER			2000
#define ROOT_ELECTION_TIME_MIN		1500
#define ROOT_ELECTION_TIME_MAX		2500
#define ROOT_ELECTION_MIN			16000
#define ROOT_ELECTION_MAX			21000
#define LEVEL_REQUEST_TIMEOUT_MIN	500
#define LEVEL_REQUEST_TIMEOUT_MAX	750
#define MY_NODE_ID AM_BROADCAST_ADDR 							// SØG og ERSTAT på MY_NODE_ID -> AM_BROADCAST

#define I_AM_ROOT 			0 // THIS IS ONLY USED IN A DEMOSTRATION. Made to insure that the root stays root even though the power source is removed.
#define DO_TEST 			5 // THIS IS ONLY USED IN TEST. 
		// - if = 0, synchronizing is done.
		// - If = 1, the clock is synchronized and T1,T2,T3,T4,clk Offset and propagation delay is simply send through serial communication.  
		// - If = 2, T1, T2, T3 and T4 is send to serial before and after calculations. 
		// - if = 3, T1, T2, T3 and T4 is send to serial only after synchronization.
		// - if = 4, Send global clock for both nodes. 
		// - if = 5, Version with root election process

module TPSN2M @safe()
{	
	uses interface Boot;
	uses interface Leds;  
	uses interface Random;
	uses interface ParameterInit<uint16_t> as Seed;
	uses interface PacketTimeStamp<TMicro, uint32_t> as TimeStamp; 
	//Timer
	uses interface Timer<TMilli> as timerLockLevel;
	uses interface Timer<TMilli> as InterfaceTimer;
	uses interface Timer<TMilli> as SyncWait;
	uses interface Timer<TMilli> as SyncBackOff;
	uses interface Timer<TMilli> as MinTimeToResync;
	uses interface Timer<TMilli> as InterfaceTimer2;
	uses interface Timer<TMilli> as LevelRequestTimeOut;
	uses interface Timer<TMilli> as TestTimer;
	uses interface Timer<TMilli> as timerLevelRetransmission;
	uses interface Timer<TMilli> as ResyncTimer;
	uses interface Timer<TMilli> as JoinNetworkTimer;
	uses interface Timer<TMilli> as RootElectionTimer;
	uses interface Timer<TMilli> as LevelRequestTimeOut2;
	uses interface Timer<TMilli> as LowestLevelReceivedTimer;
	uses interface LocalTime<TMicro>;
	
	//Radio
	uses interface Packet as RadioPacket;
	uses interface AMPacket as RadioAMPacket;
	uses interface AMSend as RadioSend;
	uses interface Receive as RadioReceive;
	uses interface SplitControl as RadioControl;
	
	// External interrupt
	uses interface HplAtm128Interrupt;	
}

implementation
{
	//Prototypes
	void Sync_GoToState(int8_t newState);
	void Sync_StartStateMachine();
	void CreateSpanningTree(bool rootNode, uint8_t sendersLevel);
	
	//Global variables
	bool serialLocked = 0;	//indicates if the serialport is locked or not
	bool radioLocked = 0;   //indicates if the radio is locked or not
	bool readyToResync = 1; // Initiates to 1;
	bool synchronized = 0; // Initiates to 0
	//bool holdProgram = 0; 
	bool nACK_Send = 0;
	bool nACK_Received = 0;
	bool SDFlimit =TRUE;
	bool sendPeriodicSync = 0;
	uint8_t resyncTimerCount = 0;
	// Added
	uint32_t ownOffset = 0;
	uint32_t localTime = 0;
	int32_t propDelayGlobal = 0;
	
	message_t serialpacket; 	// The serial communication message
	message_t serialAckPacket;
	message_t radiopacket;		// The radio message
	
 	syncInfo1_t syncInfo1;
 	syncInfo2_t syncInfo2;
	
 	uint32_t globalTime = 0;
 	uint8_t nOverflow = 0;
 	
	uint8_t treeLevel = 254; // nodes are initially set to a treelevel of 254.
	uint8_t lowestReceivedLevel = 254;
	uint8_t showTreeLevel = 0;
	uint8_t lowestRequestedLevel = 254; 
	uint8_t nRxAckLevelReqs = 0;
	uint8_t syncState = 0;	// The global syncStat used in the synchronization state machine
	uint8_t nSyncPulses = 0;  
	uint8_t nBackOffs = 0;
	uint8_t serialFailedCounter =0;		
	uint8_t nPresses = 0;

	//Only for tests
	uint8_t testCounter = 0;
	uint8_t tester1 = 0;
	uint8_t tester2 = 0;
	uint8_t tester3 = 0;
	uint8_t tester4 = 0;
	uint32_t tester_T1;
	uint32_t tester_T2;
	uint32_t tester_T3;
	uint32_t tester_T4;	
	bool testBool1 = TRUE;
	bool testBool2 = 0;
	bool testBool3 = 0;
	
	struct levelAssigned{
		bool Assigned;
		uint16_t LockCount;
	} levelAssigned;

	bool randomwait = FALSE;
	
	event void Boot.booted()
	{
		#ifdef USEPRINTF
		printf("---Boot.booted()---\n");
		printfflush();			
		#endif
		
		// Set up external interrupt
    	call HplAtm128Interrupt.disable();
		call HplAtm128Interrupt.clear();
		call HplAtm128Interrupt.edge(TRUE); // edge triggered, low to high
		call HplAtm128Interrupt.enable();
		
		call ResyncTimer.startPeriodic(RESYNC_PERIODIC_TIMER);
		levelAssigned.Assigned=FALSE;
		call RadioControl.start(); 		// Start radio port		
		
		if (DO_TEST == 5)
		{
			call JoinNetworkTimer.startOneShot(JOIN_NETWORK_TIMER);
		}
		// For demonstation purpose, so that the batteries can be removed form the root node
		if (I_AM_ROOT == 1)
		{
			treeLevel = 0;
			synchronized = 1;
			Sync_GoToState(SSM_BaseState);
		}	
		else
		{
			Sync_StartStateMachine();	
		}			
	}

	/////////////////////////////// Button interface /////////////////////////////////////////
	//  nPresses 	Leds	Function
	//	1. 			100		Show treelevel of node.
	//	2.			010		Resync (Only root node)
	//	3. 			110		Level discovery (set node as root and set level of all network)
	//	4. 			001		Node is set to tree level 0 -> BaseState
	//	5. 			101		Node is set to tree level 1 -> BaseState
	//	6. 			011		Node is set to tree level 2 -> BaseState
	//	7 			111		Node is set to tree level 3 -> BaseState 
	//	8. 			000		Node sends a level request.
	
	//Set leds according to the number of button presses
	void buttonInterfaceLEDS()
	{
		uint8_t nOptions = 9;
		nPresses = ((nPresses-1) % nOptions)+1;
		call Leds.set(nPresses);
		if (nPresses == 9)
		{
			call Leds.set(4);
		}
	}

	void ShowTreeLevelFunc()
	{
		showTreeLevel++;
		switch (showTreeLevel)
		{
			case 1:
				call InterfaceTimer2.startOneShot(INTERFACE_TIMEOUT2);
				call Leds.set(0xFF);
				break;
			case 2:
				call InterfaceTimer2.startOneShot(INTERFACE_TIMEOUT2);
				call Leds.set(treeLevel);
				break; 
			case 3:
				call InterfaceTimer2.startOneShot(INTERFACE_TIMEOUT2);
				call Leds.set(0xFF);
				break;
			case 4:
				call InterfaceTimer2.startOneShot(INTERFACE_TIMEOUT2);
				if (synchronized == 0)
					call Leds.set(1); // Red light -> Not Synchronized.
				else
					call Leds.set(2); // Green light -> Synchronized. 
				break; 
			case 5:
				call InterfaceTimer2.startOneShot(INTERFACE_TIMEOUT2);
				call Leds.set(0xFF);
				break;
			case 6:
				showTreeLevel=0;
				call Leds.set(syncState);
				break;			
		}
	}
	
	void buttonInterfaceAction(uint8_t nPressesAction)
	{
		nPresses = 0;
		switch (nPressesAction)
		{
			case 1:
				#ifdef USEPRINTF
				printf("ShowTreeLevelFunc()\n");
				printfflush();
				#endif
				// Show treelevel of node
				ShowTreeLevelFunc();
				break;
			case 2:				
				// Resync (Only root node) else do nothing
				if (treeLevel == 0)
				{
					#ifdef USEPRINTF
					printf("Sync_GoToState(SSM_RootStartResyncState)\n");
					printfflush();
					#endif
					Sync_GoToState(SSM_RootStartResyncState);
				}					
				break;
			case 3:
				#ifdef USEPRINTF
				printf("CreateSpanningTree(TRUE,255)\n");
				printfflush();
				#endif
				// Level discovery (set node as root and set level of all network)
				CreateSpanningTree(TRUE,255);
				break;
			case 4:
				#ifdef USEPRINTF
				printf("Sync_GoToState(SSM_BaseState), treeLevel = 0\n");
				printfflush();
				#endif
				// Node sættes til tree level 0 -> BaseState
				treeLevel = 0;
				Sync_GoToState(SSM_BaseState);
				break;
			case 9:
				#ifdef USEPRINTF
				printf("Sync_GoToState(SSM_BaseState), treeLevel = 1\n");
				printfflush();
				#endif
				// Node sættes til tree level 1 -> BaseState
				treeLevel = 1;
				synchronized = 0;
				Sync_GoToState(SSM_BaseState);
				break;
			case 6:
				#ifdef USEPRINTF
				printf("Sync_GoToState(SSM_BaseState), treeLevel = 2\n");
				printfflush();
				#endif
				// Node sættes til tree level 2 -> BaseState
				treeLevel = 2;
				synchronized = 0;
				Sync_GoToState(SSM_BaseState);
				break;
			case 7:
				#ifdef USEPRINTF
				printf("Sync_GoToState(SSM_BaseState), treeLevel = 3\n");
				printfflush();
				#endif
				// Node sættes til tree level 3 -> BaseState
				treeLevel = 3;
				synchronized = 0;
				Sync_GoToState(SSM_BaseState);
				break;
			case 8: 
				#ifdef USEPRINTF
				printf("Sync_GoToState(SSM_LevelReqState)\n");
				printfflush();
				#endif
				// Send Level Request
				Sync_GoToState(SSM_LevelReqState);
				break;
			case 5:
				#ifdef USEPRINTF
				printf("sendPeriodicSync = %d\n", sendPeriodicSync);
				printfflush();
				#endif
				// Send Level Request
				if (sendPeriodicSync == 0)
					sendPeriodicSync = 1;
				else
					sendPeriodicSync = 0;	
				break; 				
		}
	}
	
	//Timers to handle the button and Leds
	event void InterfaceTimer.fired() { buttonInterfaceAction(nPresses); }
	event void InterfaceTimer2.fired() { ShowTreeLevelFunc(); }
	
	// Button press interrupt handler
	async event void HplAtm128Interrupt.fired()
	{
		nPresses++;
		buttonInterfaceLEDS();
		call InterfaceTimer.startOneShot(INTERFACE_TIMEOUT1);		
	}
	
	// Radio send (Returns true=success, false=failed)
	bool my_RadioSend(nx_uint8_t msgType, nx_uint8_t nodeLevel, nx_uint16_t nodeID, nx_uint32_t data1, nx_uint32_t data2)
	{
		bool transmissionSuccess = FALSE;  
		
		// If not clear to send. 
		if (radioLocked)
		{
			transmissionSuccess = FALSE;
			#ifdef USEPRINTF
			printf("my_RadioSend, serialLocked = %i, radioLocked = %i\n", serialLocked, radioLocked);
			printfflush();
			#endif	
		}
		// Clear to send.
		else
		{
			RadioMsg* btrpkt = (RadioMsg*)(call RadioPacket.getPayload(&radiopacket, sizeof (RadioMsg)));

			if (msgType == msg_SyncPuls)
			{
				#ifdef USEPRINTF
				printf("my_RadioSend, msg_SyncPuls\n");
				printfflush();
				#endif
			}

			btrpkt->messageType = msgType;
			btrpkt->nodeTreeLevel = nodeLevel;
			btrpkt->senderID = nodeID;
			btrpkt->T2 = data1;
			btrpkt->T3 = data2;
			
			if (call RadioSend.send(MY_NODE_ID, &radiopacket, sizeof(RadioMsg)) == SUCCESS) 
			{
				tester1++; //Only for test
				radioLocked = TRUE;  //Lock the radio until the transmission is done
				transmissionSuccess = TRUE;
			}
		}
		return transmissionSuccess;
	}
	
	//Super serial send function.
	bool my_SerialSend(nx_uint32_t d1, nx_uint32_t d2, nx_uint32_t d3, nx_uint32_t d4, nx_uint32_t d5, nx_uint32_t d6)
	{
		uint8_t transmissionSuccess = FALSE;		
		#ifdef NUMBERSPRINTF
		printf("%lu %lu %lu %lu %lu %lu\n", d1, d2, d3, d4, d5, d6);
		printf("Treelevel = %i\n", treeLevel);
		printfflush();
		#endif
		serialLocked = TRUE;
		transmissionSuccess = TRUE;
		return transmissionSuccess;
	}
	
	//Returns random number between lowerbound and upperbound
	uint32_t randomnumber(uint32_t lowerbound, uint32_t upperbound)
	{
		call Seed.init(call ResyncTimer.getNow());
		return (call Random.rand32() % (upperbound-lowerbound) + lowerbound);
	}	
	
	//Function to create the spanning tree.
	//rootNode is TRUE if this is the root node, otherwise FALSE
	//Senders level indicate the level of the mode that started the syncronication of this node
	void CreateSpanningTree(bool rootNode, uint8_t sendersLevel)
	{
		//If the node is the root node
		if (rootNode)
		{
			treeLevel=0;	
			//1. send package containing its level	
			my_RadioSend(msg_LevelDisc, treeLevel, TOS_NODE_ID, 0, 0);
			levelAssigned.Assigned=TRUE;	//Lock the level, so that it can't be higher than the current value
			
			if (DO_TEST != 5)
			{
				call timerLockLevel.startPeriodic(LEVEL_LOCK_TIME);
			}			
		}
		//If level is not assigned or package received from node with lower number and not root node (to avoid underflow)
		else if (((!levelAssigned.Assigned) || (sendersLevel<(treeLevel-1))) && (treeLevel!=0) ) //If note with lower level is nearby.
		{
			#ifdef USEPRINTF
			printf("CreateSpanningTree, not root, not assigned\n");
			printf("sendersLevel = %i, treeLevel = %i", sendersLevel, treeLevel);
			printfflush();
			#endif
			
			treeLevel=sendersLevel+1;
			levelAssigned.Assigned=TRUE; //Ignore all further assignments until timer timer is fired, unless a smaller number is received			
			call timerLevelRetransmission.startOneShot(randomnumber(0, 32767));
			
			// Stop the root election timer
			call RootElectionTimer.stop();
			// Stop the join network timer
			call JoinNetworkTimer.stop();
		}
		else
		{
			#ifdef USEPRINTF
			printf("CreateSpanningTree, ELSE\n");
			printfflush();
			#endif
		}
		return;
	}
	
	//unlocks the node assignment, so a new tree can be generated
	event void timerLockLevel.fired()
	{ 	
		levelAssigned.LockCount++;
		//Because of the fast timer, wait for 5 timer periods before releasing the level lock
		if (levelAssigned.LockCount >= 5)
		{
			levelAssigned.Assigned=FALSE;
			levelAssigned.LockCount=0;
			call timerLockLevel.stop();
		}
	}
	
	//When transmission is done
	event void RadioSend.sendDone(message_t *bufPtr, error_t error)
	{
		if (&radiopacket == bufPtr)
		{
			radioLocked = FALSE;
		}

		// Only for test.
		if ((testBool3 == 1) && (DO_TEST == 4))
		{
			testBool3 = 0;
			if (my_SerialSend(globalTime, 0, 0, 0, 0, serialFailedCounter)==0)
				serialFailedCounter++;					
		}
				
		if ((SSM_SendAckState == syncState) && (nACK_Send == 1))
		{
			// Added: if level 2 wants to sync with level 1
			if (treeLevel != 0)
			{
				syncInfo1.T3 = call TimeStamp.timestamp(bufPtr) + ownOffset + propDelayGlobal;
			}
			else
			{
				syncInfo1.T3 = call TimeStamp.timestamp(bufPtr);
			}					 
			Sync_GoToState(SSM_SendAckState);
		}
		
		if (SSM_SendSyncPulseState == syncState)
		{
			syncInfo2.T1 = call TimeStamp.timestamp(bufPtr);			
		}
	}

	//When receiving
	event message_t * RadioReceive.receive(message_t *msgPtr, void *payload, uint8_t len)
	{
		// Take message payload as radio struct (RadioMsg).
		RadioMsg* btrpkt = (RadioMsg*)payload;
		//my_SerialSend(TOS_NODE_ID, treeLevel, 0, 0);
		// If length is not the right then return.
			
		if (len != sizeof(RadioMsg))
		{
			return msgPtr;
		}
		
		switch (btrpkt->messageType)
		{
			case msg_StartSync:
				// The root node sends a timePulse packet. Goes to wait state before sending a timePulse  
				if ((syncState == SSM_BaseState)&&(btrpkt->nodeTreeLevel == 0)&&(treeLevel == 1)&&(readyToResync ==1))
				{					
					// Added: start root election timer
					if (DO_TEST == 5)
					{
						call RootElectionTimer.startOneShot(randomnumber(ROOT_ELECTION_MIN, ROOT_ELECTION_MAX)); // can miss 2 syncs
					}					
					Sync_GoToState(SSM_WaitState);					
				}
				break;
			case msg_SyncPuls:			
				#ifdef USEPRINTF
				printf("Receive: msg_SyncPuls, syncState = %i\n", syncState);
				printfflush();
				#endif
			
				// Overhearing a message from a -1 lower level. Go to do or restart back-off state before sending a message.
				// NOTE! The BackOffState can be accessed from either BaseState or from BackOffState itself.  
				if ((btrpkt->nodeTreeLevel == (treeLevel-1))&&((syncState == SSM_BaseState)||(syncState == SSM_BackOffState))&&(readyToResync == 1))
				{
					Sync_GoToState(SSM_BackOffState);
				}			
				
				// A sync packet has been received from the +1 higher level. Send an Ack message.
				if ((btrpkt->nodeTreeLevel == (treeLevel+1))&&(syncState == SSM_BaseState)&&(synchronized == 1))
				{
					//syncInfo.nodeTreeLevel = treeLevel;					
					
					// Added: if level 2 wants to sync with level 1
					if (treeLevel != 0)
					{
						syncInfo1.T2 = call TimeStamp.timestamp(msgPtr) + ownOffset + propDelayGlobal;			
					}
					else
					{
						syncInfo1.T2 = call TimeStamp.timestamp(msgPtr);	
					}
					
					syncInfo1.senderID = btrpkt->senderID;
					Sync_GoToState(SSM_SendAckState);
				}
				break;  			
			case msg_Ack:			
				#ifdef USEPRINTF
				printf("Receive: msg_Ack, syncState = %i\n", syncState);
				printfflush();
				#endif
			
				// The first ack is accepted if the treeLevel is -1 and the node is in SendSyncPulseState. 
				// TODO Det skal undersøges om senderID er rigtig. Skal testes!! 
				if ((btrpkt->nodeTreeLevel == (treeLevel-1))&&(syncState == SSM_SendSyncPulseState))
				{	
					syncInfo2.T4 = call TimeStamp.timestamp(msgPtr);
					Sync_GoToState(SSM_AckReceivedState);
				}
				
				// The second ack is accepted if the treeLevel is -1 and the node is in AckReceivedState.
				// TODO Det skal undersøges om senderID er rigtig. Skal testes!!
				else if ((btrpkt->nodeTreeLevel == (treeLevel-1))&&(syncState == SSM_AckReceivedState))
				{
					syncInfo2.T2 = btrpkt->T2;
					syncInfo2.T3 = btrpkt->T3;
					testBool3 = 1;
					Sync_GoToState(SSM_SynchronizeNodeState);
				}				
				break;
			case msg_LevelRequest:
				#ifdef USEPRINTF
				printf("Receive: msg_LevelRequest\n");
				printfflush();
				#endif
				Sync_GoToState(SSM_RxLevelReqState);
				break;
			case msg_AckLevelRequest:
				#ifdef USEPRINTF
				printf("Receive: msg_AckLevelRequest\n");
				printfflush();
				#endif
				
				// Stop the root election timer
				call RootElectionTimer.stop();
				// Stop the join network timer
				call JoinNetworkTimer.stop();
				
				if (DO_TEST == 5 && !levelAssigned.Assigned)
				{
					// Set the tree level of this node to the received tree level + 1
					call LowestLevelReceivedTimer.startOneShot(200);
					if (lowestReceivedLevel > btrpkt->nodeTreeLevel)
					{
						lowestReceivedLevel = btrpkt->nodeTreeLevel + 1;
					}
				}
				else
				{
					nRxAckLevelReqs++; 
					if (btrpkt->nodeTreeLevel < lowestRequestedLevel)
					{
						lowestRequestedLevel = btrpkt->nodeTreeLevel;
					}						
				}	
				break;
			case msg_LevelDisc:
				#ifdef USEPRINTF
				printf("Receive: msg_LevelDisc\n");
				printfflush();
				#endif
				// Stop the root election timer
				call RootElectionTimer.stop();
				// Stop the join network timer
				call JoinNetworkTimer.stop();
				//A level update package has been received
				CreateSpanningTree(FALSE,btrpkt->nodeTreeLevel);
				break;	
			default:
				return msgPtr;
				break;
		}
		return msgPtr;
	}
	
	/////////////////////////// Sync State-Machine ///////////////////////////////
	void SynchronizeTime(float T1, float T2, float T3, float T4)
	{
		float clkOffSet;
		float propDelay;
		uint32_t newTime;
		
		#ifdef USEPRINTF
		printf("SynchronizeTime()\n");
		printfflush();
		#endif
		
		/////////////////////// TEST ///////////////////////////////////
		testBool2 = 1;
		tester_T1 = (uint32_t)(T1);
		tester_T2 = (uint32_t)(T2);
		tester_T3 = (uint32_t)(T3);
		tester_T4 = (uint32_t)(T4);
		///////////////////////////////////////////////////////////////
		
		// If an overflow has occured between T1 and T4.
		if (T1 > T4)
		{
			T4 = T4+16777216;	
		}
			
		// If an overflow has occured between T2 and T3.
		if (T2 > T3)
		{
			T3 = T3+16777216;
		}
		
		// T2 and T3 will always be bigger than T1 and T4. -> Offset will be positive.
		if (T1 > T2)
		{
			T2 = T2+16777216;
			T3 = T3+16777216;
		}
    
		clkOffSet = ((T2-T1)-(T4-T3))/2;
		propDelay = ((T2-T1)+(T4-T3))/2;
		
		ownOffset = (uint32_t)clkOffSet;
		propDelayGlobal = (int32_t)propDelay;
		// This localTime should be the value that we should use to update the mote's own time. If we could.
		localTime = call LocalTime.get() + ownOffset + propDelayGlobal;		
		
		// Ensure that the propagation delay is not too large. If it is, some error must have occured, and we just set it to 0. 		
		if (propDelayGlobal > 1000 || propDelayGlobal < -1000)
		{
			propDelayGlobal = 0;
		}
		
		if (DO_TEST == 3 || DO_TEST == 5)
		{			
			my_SerialSend((uint32_t)(T1), (uint32_t)(T2), (uint32_t)(T3), (uint32_t)(T4), (uint32_t)(clkOffSet), localTime);			
		}
		else	// Under normal conditions the timer is synchronized.
		{	 		
			if ((propDelay > -1000) && (propDelay < 1000))	// If to high propgation value is calculated an error has occured.
			{	 
			
				// Sync timer
				newTime = (uint32_t)(1)+(uint32_t)(clkOffSet);
				//TBR = (uint16_t)(newTime); // This is okay because clkOffSet is always positive, and because values above 16-bits is ignored from the 16-bit timer.
				nOverflow = nOverflow+(uint8_t)((uint32_t)(newTime)>>16);
				
				
				
				
	
				if (DO_TEST == 1)
				{
					if (my_SerialSend((uint32_t)(T1), (uint32_t)(T2), (uint32_t)(T3), (uint32_t)(T4),(uint32_t)(clkOffSet),globalTime)==0)
						serialFailedCounter++;
				}	
				if (DO_TEST == 2)
				{
					if (my_SerialSend((uint32_t)(T1), (uint32_t)(T2), (uint32_t)(T3), (uint32_t)(T4),(uint16_t)(newTime),(uint8_t)((uint32_t)(newTime)>>16))==0)
						serialFailedCounter++;					
				}	
				if (DO_TEST == 4)
				{
					if (my_SerialSend(globalTime, 0, 0, 0,0, serialFailedCounter)==0)
						serialFailedCounter++;					
				}
			}
			else
			{
					// Do new synchronization? 
			}	
		}
		synchronized = 1;
	}
	
	//Function for random delay
	void WaitRandom(uint32_t waitTime)
	{
		call SyncWait.startOneShot(randomnumber(0,waitTime)); // Waits between 0 and WAIT_TIME ms.			
	}
	
	void waitTimeOut()
	{
		if (syncState == SSM_SendSyncPulseState)
		{
			nSyncPulses++;
			//call Leds.set(nSyncPulses);
		}
		// Wait random time.
		call SyncWait.startOneShot(randomnumber(SYNC_TIMEOUT-SYNC_TIMEOUT/3  ,  SYNC_TIMEOUT+SYNC_TIMEOUT/3));
	}
	
	event void SyncWait.fired()
	{
		nACK_Send = 0;
		// Send timeSync
		if (syncState == SSM_WaitState)
		{
			Sync_GoToState(SSM_SendSyncPulseState);
		}
		// No ACK received, resend syncPulse
		else
		{
			if (nSyncPulses > nTrialsMax)
			{
				#ifdef USEPRINTF
				printf("SyncWait.fired(), -> SSM_LevelReqState, nSyncPulses = %i\n", nSyncPulses);
				printfflush();
				#endif
				//Sync_GoToState(SSM_LevelReqState); // Send level_req_ack. Failed to receive ACK after nTrialsMax -> send level request.
				Sync_GoToState(SSM_Mode5_LevelReqState); // Send level_req_ack. Failed to receive ACK after nTrialsMax -> send level request.  
			}	
			else
			{
				Sync_GoToState(SSM_SendSyncPulseState);	// Message is resent	
			}				
		}
	}
	
	//Backoff and count the number of backoffs
	void DoBackOff()
	{
		nBackOffs++;
		if (nBackOffs > 1)
		{
			//call Leds.set(nBackOffs);
		}		
		// Do a backoff for a random time
		call SyncBackOff.startOneShot(randomnumber(BACK_OFF_TIME-BACK_OFF_TIME/3  ,  BACK_OFF_TIME+BACK_OFF_TIME/3));
	}
	
	event void SyncBackOff.fired()
	{
		Sync_GoToState(SSM_SendSyncPulseState);
	}
	
	event void LevelRequestTimeOut.fired()
	{		
		#ifdef USEPRINTF
		printf("LevelRequestTimeOut.fired()\n");
		printfflush();
		#endif		
		if (nRxAckLevelReqs == 0)
		{	// No level request ack has been received.		
			// Node failed to find any other node.	
			treeLevel = 254;
			lowestRequestedLevel = 254;
			Sync_StartStateMachine();	// The Synchronizing is reset. 			
		}
		else
		{
			nRxAckLevelReqs = 0; // nRxLevelReqs is reset. 					
			treeLevel = lowestRequestedLevel+1;
			lowestRequestedLevel = 254; // lowestRequestedLevel is reset.
			Sync_StartStateMachine();	// The Synchronizing is reset.			
		}		
		#ifdef USEPRINTF
		printf("LevelRequestTimeOut.fired(), treeLevel = %i\n", treeLevel);
		printfflush();
		#endif
	}
	
	//When the motes are ready for a new resynchronization
	event void MinTimeToResync.fired()
	{
		readyToResync = 1;
	}
	
	//
	// Statemachine for the time synchronization phase
	//
	void Sync_StateMachine()
	{
		switch (syncState)
		{
			case SSM_BaseState:
				// Do nothing
				break;
			case SSM_WaitState:
				#ifdef USEPRINTF
				printf("SSM_WaitState\n");
				printfflush();
				#endif
				synchronized = 0;
				// Wait random
				WaitRandom(WAIT_RANDOM_TIME);
				break;  			
			case SSM_BackOffState:
				#ifdef USEPRINTF
				printf("SSM_BackOffState\n");
				printfflush();
				#endif
				synchronized = 0;
				// Do Back off.
				DoBackOff();
				break;
			case SSM_SendSyncPulseState:	
				#ifdef USEPRINTF
				printf("SSM_SendSyncPulseState\n");
				printfflush();
				#endif		
				nBackOffs = 0;
				waitTimeOut(); // Starts a timer. If no ACK is received a TimeOut is made and the SyncPulse will be send again.
				my_RadioSend(msg_SyncPuls, treeLevel, TOS_NODE_ID, 0, 0);				
				
				// T1 is stored for later synchronization
				//syncInfo2.T1 = globalTime;
				break;
			case SSM_SynchronizeNodeState:
				#ifdef USEPRINTF
				printf("SSM_SynchronizeNodeState\n");
				printfflush();
				#endif
				call SyncWait.stop(); // The timer is stopped. No timeout will occure. 
				nBackOffs = 0;
				nSyncPulses = 0;
				
				// Time synchronization is performed.
				SynchronizeTime(syncInfo2.T1, syncInfo2.T2, syncInfo2.T3, syncInfo2.T4);
				readyToResync = 0;				
				call MinTimeToResync.startOneShot(MIN_TIME_BETWEEN_RESYNC);
				Sync_GoToState(SSM_BaseState);
				break;
			case SSM_SendAckState:
				#ifdef USEPRINTF
				printf("SSM_SendAckState\n");
				printfflush();
				#endif
				// T2 and T3 is measured. 
				if (nACK_Send == 0)
				{ // Send first Ack
					nACK_Send++;
					//syncInfo1.T2 = globalTime;
					if (my_RadioSend(msg_Ack, treeLevel, syncInfo1.senderID, 2, 3) == 1)
					{
						tester2++; // Test: if success count one up.	
					}								
				}
				else
				{	// Send second Ack
					nACK_Send = 0;
					if (my_RadioSend(msg_Ack, treeLevel, syncInfo1.senderID,syncInfo1.T2, syncInfo1.T3) == 1)
					{
						tester3++; // Test: if success count one up.	
					}						
					tester4++;					
					Sync_GoToState(SSM_BaseState);			
				}
				break;
			case SSM_RootStartResyncState: // 
				#ifdef USEPRINTF
				printf("SSM_RootStartResyncState\n");
				printfflush();
				#endif
				synchronized = 1;
				my_RadioSend(msg_StartSync, treeLevel, TOS_NODE_ID, 0, 0);
				// It is assumed that timesync has been transfered without erros. Error check should be done in case of futher development

				Sync_GoToState(SSM_BaseState);
				break;
			case SSM_AckReceivedState:
				#ifdef USEPRINTF
				printf("SSM_AckReceivedState\n");
				printfflush();
				#endif
				// Timestamp of the first AckPacket. 
				// syncInfo2.T4 = globalTime; 
				break;
			case SSM_LevelReqState:
				#ifdef USEPRINTF
				printf("SSM_LevelReqState\n");
				printfflush();
				#endif
				// Get a new tree level to the node. 
				nSyncPulses = 0;
				my_RadioSend(msg_LevelRequest, treeLevel, 0, 0, 0); // Send level request
				call LevelRequestTimeOut.startOneShot(LEVEL_REQUEST_TIMEOUT);
				break;
			case SSM_RxLevelReqState:
				#ifdef USEPRINTF
				printf("SSM_RxLevelReqState\n");
				printfflush();
				#endif				
				if (DO_TEST == 5)
				{
					call LevelRequestTimeOut2.startOneShot(randomnumber(LEVEL_REQUEST_TIMEOUT_MIN, LEVEL_REQUEST_TIMEOUT_MAX));
				}
				else
				{
					// Send Ack Level Request = send treeLevel.
					my_RadioSend(msg_AckLevelRequest, treeLevel, 0, 0, 0);
					Sync_GoToState(SSM_BaseState);
				}				
				break;
			case SSM_Mode5_LevelReqState:
				#ifdef USEPRINTF
				printf("SSM_Mode5_LevelReqState\n");
				printfflush();
				#endif
				nSyncPulses = 0;
				my_RadioSend(msg_LevelRequest, treeLevel, 0, 0, 0); // Send level request
				break;
			default:
				return;
				break;
		}
	}
	
	void Sync_StartStateMachine()
	{
		readyToResync = 1;
		synchronized = 0; 
		Sync_GoToState(SSM_BaseState); 	
	}
	
	//Change state depending on the current one
	void Sync_GoToState(int8_t newState)
	{
		syncState = newState;		
		call Leds.set(newState);
		Sync_StateMachine();
	}	
	
	event void timerLevelRetransmission.fired()
	{
		my_RadioSend(msg_LevelDisc, treeLevel, TOS_NODE_ID, 0, 0);	
		//Make reassignment possible again after a given time
		call timerLockLevel.startPeriodic(LEVEL_LOCK_TIME);
	}

	event void ResyncTimer.fired()
	{
		if ((sendPeriodicSync == 1) && (treeLevel == 0))
		{
			#ifdef USEPRINTF
			printf("ResyncTimer.fired(), sendPeriodicSync=1 og ROOT\n");
			printfflush();
			#endif
			if (resyncTimerCount > 3)
			{
				resyncTimerCount = 0;
				#ifdef USEPRINTF
				printf("Sync_GoToState(SSM_RootStartResyncState)\n");
				printfflush();
				#endif
				Sync_GoToState(SSM_RootStartResyncState);
			} 
			else
			{
				resyncTimerCount++;	
			}
		}
	}

	event void RootElectionTimer.fired()
	{
		#ifdef USEPRINTF
		printf("RootElectionTimer.fired()\n");
		printfflush();
		#endif
		// The node is now root, and it should create the spanning tree.
		CreateSpanningTree(TRUE, 255);		
		
		// Start the resync timer after the tree has been made.
		sendPeriodicSync = 1;
		call ResyncTimer.startPeriodic(RESYNC_PERIODIC_TIMER);
	}

	event void JoinNetworkTimer.fired()
	{
		#ifdef USEPRINTF
		printf("JoinNetworkTimer.fired()\n");
		printfflush();
		#endif
		// Send Level Request
		//Sync_GoToState(SSM_LevelReqState);
		Sync_GoToState(SSM_Mode5_LevelReqState);
		// Start the root election timer after a random time between 1.5 s and 2.5 s.
		call RootElectionTimer.startOneShot(randomnumber(ROOT_ELECTION_TIME_MIN, ROOT_ELECTION_TIME_MAX));
	}

	event void LevelRequestTimeOut2.fired()
	{
		// Send Ack Level Request = send treeLevel.
		my_RadioSend(msg_AckLevelRequest, treeLevel, 0, 0, 0);
		Sync_GoToState(SSM_BaseState);
	}

	event void LowestLevelReceivedTimer.fired()
	{
		treeLevel = lowestReceivedLevel;
		lowestReceivedLevel=254;
		levelAssigned.Assigned = TRUE;
		Sync_StartStateMachine();
	}
	
	// Empty events
	event void RadioControl.startDone(error_t error){}
	event void RadioControl.stopDone(error_t error){}
	event void TestTimer.fired(){}
}